/*
 * File:    Defines_GroundLink.c
 * Project:	HobbyPilotFirmware (HPF)
 * Author:  Wouter van Verre
 *
 * Created on February 20, 2011, 5:17 PM
 */

#include "Header.h"

/** GroundLink Parameter List **/
const GroundLinkParameter GroundLinkParameterList[] = 
{
	{"Reserved", NULL, NULL, READONLY},
	{"Rotation Matrix", &GL_Send_RotationMatrix, NULL, READONLY},
	{"CPU Load Info", &GL_Send_CPULoadInfo, NULL, READONLY}
};

/** Parameter Functions **/
void GL_Send_RotationMatrix(void)
{
	uint16 checksum;

	uint8 msgHeader[] = {0x55,
					  	  37, 
						  GLCompIMUSendSeq++, 
						  GL_AIRCRAFT_ID, 
						  GL_COMP_IMU_ID, 
						  GL_MSG_SENDPARAM_ID,
						  GL_PARAM_ROTATIONMATRIX_ID};

	crc_init(&checksum);
	crc_calculate((uint8*)&msgHeader[1], (sizeof(msgHeader) / sizeof(uint8)) - 1, &checksum);
	crc_calculate((uint8*)&m3x3_Rotation[0], (sizeof(m3x3_Rotation) / sizeof(uint8)), &checksum);

	writeToCircularBuffer(&UART1TXBuffer, msgHeader, (sizeof(msgHeader) / sizeof(uint8)));
	writeToCircularBuffer(&UART1TXBuffer, (uint8*)&m3x3_Rotation[0], (sizeof(m3x3_Rotation) / sizeof(uint8)));
	writeToCircularBuffer(&UART1TXBuffer, ((uint8*)&checksum), (sizeof(checksum) / sizeof(uint8)));
	writeToUART1();
}

void GL_Send_CPULoadInfo(void)
{
	uint16 checksum;

	uint8 Message[] = {0x55,
					  	 7, 
						 GLCompIMUSendSeq++, 
						 GL_AIRCRAFT_ID, 
						 GL_COMP_IMU_ID, 
						 GL_MSG_SENDPARAM_ID,
						 GL_PARAM_CPULOADINFO_ID,
						 cpuLoad,
						 cpuLoadMax,
					     ((uint8*)&cpuStackUse)[0],
						 ((uint8*)&cpuStackUse)[1],
						 ((uint8*)&cpuStackUseMax)[0],
						 ((uint8*)&cpuStackUseMax)[1]};

	crc_init(&checksum);
	crc_calculate((uint8*)&Message[1], (sizeof(Message) / sizeof(uint8)) - 1, &checksum);

	writeToCircularBuffer(&UART1TXBuffer, Message, (sizeof(Message) / sizeof(uint8)));
	writeToCircularBuffer(&UART1TXBuffer, ((uint8*)&checksum), (sizeof(checksum) / sizeof(uint8)));
	writeToUART1();
}
